from machine import I2C, UART, Pin, PWM, Timer, ADC

from DShotPIO import DShotPIO, DSHOT_SPEEDS
from mpudmp import MPU6050
import time

i2c = I2C(id=1, sda=Pin(14), scl=Pin(15), freq=2000_000)

device_address = 0x68
freq_divider = 0x03  # freq = 200Hz / (1 + value)  推荐 >=4

# Make an MPU6050
mpu = MPU6050(i2c, device_address, freq_divider)

# Initiate your DMP
mpu.dmp_initialize()
mpu.set_DMP_enabled(True)

packet_size = mpu.DMP_get_FIFO_packet_size()
FIFO_buffer = [0]*64

global pitch, roll, yaw, gyro_x,gyro_y,gyro_z, dpsCounter

fifoFull = Pin(16, Pin.IN)

lastime = 0

while True:
    
    
    
    #while not mpu.isreadyFIFO(packet_size): # Check if FIFO data are ready to use...
    #    pass
    
    #print(pinInt.value())
    
    if (fifoFull.value()):
        
        #print(time.ticks_ms() - lastime)
        #lastime = time.ticks_ms()
        FIFO_buffer = mpu.get_FIFO_bytes_block(packet_size) # get all the DMP data here
        #print(time.ticks_ms() - lastime)

        gyro_x,gyro_y,gyro_z = mpu.DMP_get_gyro_int16(FIFO_buffer)

        qw,qx,qy,qz = mpu.DMP_get_quaternion_int16(FIFO_buffer)
        #grav_x,grav_y,grav_z = mpu.DMP_get_gravity(qw,qx,qy,qz)
        roll, pitch, yaw = mpu.DMP_get_euler_roll_pitch_yaw(qw,qx,qy,qz)

        lastime = time.ticks_ms()
       
        print("roll:{}, pitch:{}, yaw:{}".format(roll, pitch, yaw))
        

